/*
 * Copyright 2025 The Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 *@file    : se2.h
 *@brief   : se2 header 
 *@authors : zhanglei, zhanglei_723@126.com
 *@version : v1.0
 *@data    : 2025/8/24
 *
 */

#ifndef SE2_H
#define SE2_H

#include <Eigen/Dense>

#include "common/so2.h"

using namespace std;

class SE2 {
public:
	SE2();
	SE2(const double& x, const double& y, const double& t);
	SE2(const SO2& so2, const Eigen::Vector2d& p);
	SE2 Inv() const;

	SE2 operator +(const SE2& that) const;
	SE2 operator -(const SE2& that) const;

	const Eigen::Vector2d & translation() const {
    	return p_;
  	}

  	Eigen::Vector2d& translation() {
    	return p_;
  	}

  	const SO2& so2() const{
  		return so2_;
  	}
  	
  	SO2& so2() {
  		return so2_;
  	}
  	Eigen::Matrix3d matrix() const;
  	static Eigen::Matrix4d matrix4d(const SO2& so2, const Eigen::Vector2d& p);
  	static Eigen::Matrix3d skew(const Eigen::Vector3d& p);
private:
	SO2 so2_;
	Eigen::Vector2d p_;
};

#endif
